﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using DecodeInterface;

namespace MotionDetect
{
    public class MeanShift : DecodeBitmap
    {
        bool startTrack = false;
        bool initialTrack = false;
        Rectangle rectTrack = new Rectangle();
        double[] mouldFeature = new double[0];//模板特征权值概率分布
        static Color colorTrack = Color.Yellow;
        static int intersectNumber = 25;//与动态区域相交计数器
        static readonly Size SizePadding = new Size(50, 50);        //内边框有效距离
        Random random = new Random();
        Rectangle rectAvailable;

        protected override unsafe object[] OnDecode(byte* pIn, byte* pOut, Size sizeIn, Size sizeOut, params object[] state)
        {
            if (state.Length < 4)
                throw new ApplicationException("third object and fourth object of state is not exsit|ParticleFilter");

            List<Rectangle> rectMotion = state[2] as List<Rectangle>;
            Rectangle rectMax = (Rectangle)state[3];

            if (rectMax != Rectangle.Empty && !startTrack)
            {
                startTrack = true;
                rectTrack = rectMax;
            }

            //新空图像
            for (int yOut = 0; yOut < sizeOut.Height; yOut++)
            {
                for (int xOut = 0; xOut < sizeOut.Width; xOut++)
                {
                    int index = DataManager.GetIndex(xOut, yOut, sizeOut.Width);
                    pOut[index] = pOut[index + 1] = pOut[index + 2] = 0;
                }
            }

            if (startTrack)
            {//开始跟踪
                if (!initialTrack)
                {//初始化跟踪
                    mouldFeature = GetFeatureWeight(rectTrack, pIn, sizeIn);
                    rectAvailable = new Rectangle(new Point(), sizeOut);
                    rectAvailable.Inflate(-SizePadding.Width, -SizePadding.Width);
                    initialTrack = true;
                }
                else
                {
                    Point center = new Point(
                        rectTrack.Left + rectTrack.Width / 2,
                        rectTrack.Top + rectTrack.Height / 2);
                    double[] centerFeature = GetFeatureWeight(rectTrack, pIn, sizeIn);

                    Point newCenter = GetNextPoint(rectTrack, pIn, sizeIn, mouldFeature, centerFeature);
                    while (center != newCenter)
                    {
                        center = newCenter;
                        rectTrack.Location = new Point(
                            center.X - rectTrack.Width / 2,
                            center.Y - rectTrack.Height / 2);
                        if (!rectAvailable.Contains(rectTrack)) break;
                        centerFeature = GetFeatureWeight(rectTrack, pIn, sizeIn);
                        newCenter = GetNextPoint(rectTrack, pIn, sizeIn, mouldFeature, centerFeature);
                    }

                    //检测是否与运动区域相交，如果不相交则计数器减一
                    bool isIntersect = false;
                    foreach (var item in rectMotion)
                    {
                        if (item.IntersectsWith(rectTrack))
                        {
                            isIntersect = true;
                            break;
                        }
                    }
                    if (!isIntersect) intersectNumber--;
                    //跟踪目标丢失
                    
                    if (!rectAvailable.Contains(rectTrack)
                        || intersectNumber <= 0)
                    {
                        initialTrack = false;
                        startTrack = false;
                        rectTrack = new Rectangle();
                        mouldFeature = new double[0];
                        colorTrack = Color.FromArgb(
                            random.Next(256),
                            random.Next(256),
                            random.Next(256));
                        intersectNumber = 50;
                    }
                }

                
            }
            if (startTrack)
            {
                for (int h = rectTrack.Y - 20; h < rectTrack.Y + rectTrack.Height + 20; h++)
                {
                    for (int w = rectTrack.X - 20; w < rectTrack.X + rectTrack.Width + 20; w++)
                    {
                        int index = DataManager.GetIndex(w, h, sizeOut.Width);
                        pOut[index] = pOut[index + 1] = pOut[index + 2] = pIn[index];
                    }
                }

                //绘制跟踪框
                if (rectTrack.Width > 0 && rectTrack.Height > 0)
                    DataManager.DrawRectangle(rectTrack, colorTrack, pOut, sizeOut);
                //绘制有效区域边框
                DataManager.DrawRectangle(rectAvailable, Color.Red, pOut, sizeOut);
            }

            if (this.startTrack)
                return new object[] { rectTrack };
            else 
                return new object[] { Rectangle.Empty };
        }

        //获取特征权值概率分布
        unsafe double[] GetFeatureWeight(Rectangle rectTrack, byte* pIn, Size sizeIn)
        {
            Double[] q = new double[256];
            PointF center = new PointF(
                (float)rectTrack.Left + (float)rectTrack.Width / 2,
                (float)rectTrack.Top + (float)rectTrack.Height / 2);
            SizeF H = new SizeF((float)rectTrack.Width / 2, (float)rectTrack.Height / 2);
            for (int y = rectTrack.Top; y < rectTrack.Bottom; y++)
            {
                for (int x = rectTrack.Left; x < rectTrack.Right; x++)
                {
                    int index = DataManager.GetIndex(x, y, sizeIn.Width);
                    byte u = pIn[index];
                    //计算以高斯分布为核函数自变量X的值
                    double X = ((Math.Pow((x - center.X), 2) + Math.Pow((y - center.Y), 2))
                            / (Math.Pow(H.Width, 2) + Math.Pow(H.Height, 2)));
                    //将每个特征的高斯分布核函数的值累加得出特征权值
                    q[u] += Math.Exp(-Math.Pow(X, 2) / 2) / Math.Sqrt(2 * Math.PI);
                }
            }

            double C = 0;
            //求归一化常数C
            for (int i = 0; i < 256; i++) C += q[i];
            //求归一化特征权值
            for (int i = 0; i < 256; i++) q[i] /= C;

            return q;
        }

        unsafe public Point GetNextPoint(Rectangle rectTrack, byte* pIn, Size sizeIn, double[] Qu, double[] PuY0)
        {
            double[] w = new double[256];//方向权值
            for (int i = 0; i < 256; i++)
            {
                w[i] = Math.Sqrt(Qu[i] / PuY0[i]);
            }

            PointF center = new PointF(
                (float)rectTrack.Left + (float)rectTrack.Width / 2,
                (float)rectTrack.Top + (float)rectTrack.Height / 2);
            SizeF H = new SizeF((float)rectTrack.Width / 2, (float)rectTrack.Height / 2);
            double numeratorX = 0, numeratorY = 0;//分子
            double denominatorX = 0, denominatorY = 0;//分母
            for (int y = rectTrack.Top; y < rectTrack.Bottom; y++)
            {
                for (int x = rectTrack.Left; x < rectTrack.Right; x++)
                {
                    int index = DataManager.GetIndex(x, y, sizeIn.Width);
                    byte u = pIn[index];
                    //计算以高斯分布为核函数自变量X的值
                    double X = ((Math.Pow((x - center.X), 2) + Math.Pow((y - center.Y), 2))
                            / (Math.Pow(H.Width, 2) + Math.Pow(H.Height, 2)));
                    //负的高斯分布核函数的值
                    double Gi = -Math.Exp(-Math.Pow(X, 2) / 2) / Math.Sqrt(2 * Math.PI);

                    numeratorX += x * w[u] * Gi;
                    numeratorY += y * w[u] * Gi;
                    denominatorX += w[u] * Gi;
                    denominatorY += w[u] * Gi;
                }
            }
            return new Point((int)(numeratorX / denominatorX), (int)(numeratorY / denominatorY));
        }
    }
}
